#include "Visualizer.h"
#include "stdafx.h"
#include <glut.h>
#include "World.h"
#include <ctime>
#include "Vector2.h"
#include "KalmanTank.h"
#include <iostream>
#define _USE_MATH_DEFINES
#include <math.h>

void init ( GLvoid );
void display ( void );
void arrow_keys ( int a_keys, int x, int y );
void keyboard ( unsigned char key, int x, int y );
void reshape ( int w, int h );
void MouseButton(int button, int state, int x, int y);
void MouseMotion(int x, int y);
DWORD WINAPI glutLoopAsync(LPVOID iValue);

void setColor(std::string color, float* r, float* g, float* b);
void draw_world_outline(int z_index);
void draw_bases(int z_index);
void draw_obstacles(int z_index);
void draw_my_tanks(int z_index);
void draw_other_tanks_heat_map(int z_index);
void resetCamera();

int eyex;
int eyey;
int zoom;
int clickx = 0;
int clicky = 0;
bool button_down = false;

Visualizer::Visualizer(void)
{
	resetCamera();
}

void resetCamera() {
	eyex = 0;
	eyey = 0;
	zoom = 480;
}

Visualizer::~Visualizer(void)
{
}

void Visualizer::init(int argc, char* argv[]) {
	glutInit(&argc, argv);
}

void Visualizer::show() {
	HANDLE hThread;
	DWORD dwGenericThread;
	hThread = CreateThread(NULL, 0, glutLoopAsync, NULL, 0, &dwGenericThread);
}

DWORD WINAPI glutLoopAsync(LPVOID iValue) {
	glutInitDisplayMode ( GLUT_RGBA | GLUT_DOUBLE | GLUT_DEPTH | GLUT_ALPHA ); // Display Mode
	glutInitWindowSize  ( 720, 720 );
	glutCreateWindow    ( "Grid Lab Visualization" );
	glutDisplayFunc     ( display );
	glutIdleFunc		( display );
	glutReshapeFunc     ( reshape );
	glutKeyboardFunc    ( keyboard );
	glutMouseFunc       ( MouseButton );
	glutMotionFunc      ( MouseMotion );
	glutSpecialFunc     ( arrow_keys );
	init();
	glutMainLoop        ( );             // Initialize The Main Loop
	return 0;
}

void init ( GLvoid )     // Create Some Everyday Functions
{
	glShadeModel(GL_SMOOTH);							// Enable Smooth Shading
	glClearColor(0.0f, 0.0f, 0.0f, .5f);
	glClearDepth(1.0f);									// Depth Buffer Setup
	glEnable(GL_DEPTH_TEST);							// Enables Depth Testing
	glDepthFunc(GL_LEQUAL);								// The Type Of Depth Testing To Do
	glEnable(GL_BLEND);
	glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
	
	glHint(GL_PERSPECTIVE_CORRECTION_HINT, GL_NICEST);
	srand((unsigned)time(NULL));
}

void display ( void )   // Create The Display Function
{
	glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);	// Clear Screen And Depth Buffer
	glLoadIdentity();									// Reset The Current Modelview Matrix

	gluLookAt(eyex, eyey, zoom, eyex, eyey, 0, 0, 1, 0);

	draw_world_outline(0);
	draw_bases(2);
	draw_obstacles(2);
	draw_my_tanks(4);
	draw_other_tanks_heat_map(4);

	glutSwapBuffers ( );
	// Swap The Buffers To Not Be Left With A Clear Screen
}
void setColor(std::string color, float* r, float* g, float* b) {
	if (color == "green") {
		*r = 0;
		*g = 1;
		*b = 0;
	}
	else if (color == "blue") {
		*r = 0;
		*g = 0;
		*b = 1;
	}
	else if (color == "purple") {
		*r = .625;
		*g = .125;
		*b = .9375;
	}
	else if (color == "red") {
		*r = 1;
		*g = 0;
		*b = 0;
	}
	else {
		*r = .3;
		*g = .3;
		*b = .3;
	}
}

void draw_my_tanks(int z_index) {
	World* world = World::instance();

	for (unsigned int i = 0; i < world->tanks().size(); i++) {
		tank_t* tank = &world->tanks().at(i);
		float r, g, b;
		setColor("red", &r, &g, &b);

		float x = tank->pos[0];
		float y = tank->pos[1];

		glColor4f(r, g, b, 1.0);
		glBegin(GL_QUADS);
			glVertex3f(x - 1, y - 1, z_index);
			glVertex3f(x + 2, y - 1, z_index);
			glVertex3f(x + 2, y + 2, z_index);
			glVertex3f(x - 1, y + 2, z_index);
		glEnd();
	}
}

double getValAt(Vector2 & cur_estimate, double x, double y, float sigma_x = 5, float sigma_y = 5, float rho = 0) {
	//float sigma_x = 5;
	//float sigma_y = 5;
	//float rho = 0;

	double dx = x - cur_estimate.x;
	double dy = y - cur_estimate.y;
	return 1.0 / (2.0 * M_PI * sigma_x * sigma_y * sqrt(1 - pow(rho, 2))) * exp(-1.0 / 2.0 * (pow(dx, 2) / pow(sigma_x, 2) + pow(dy, 2) / pow(sigma_y, 2) - 2.0 * rho * dx * dy / (sigma_x * sigma_y)));
}

void draw_other_tanks_heat_map(int z_index) {
	World* world = World::instance();
	int mid = world->getSize() / 2;
	int box_size = 30;

	for (unsigned int tank_idx = 0; tank_idx < world->enemies().size(); tank_idx++) {
		tank_t* myTank = &world->tanks()[0];
		otank_t* tank = &world->other_tanks()[tank_idx];
		KalmanTank* ktank = &world->enemies().at(tank_idx);

		Vector2 pos = ktank->GetCurrentEstimate();

		float t = 0.0;
		Vector2 tankP = Vector2(myTank->pos[0], myTank->pos[1]);
		Vector2 predicted;
		float shotV = World::instance()->getBulletSpeed();
		float shotT = 0.0;

		while(1){
			t += .15;
			predicted = ktank->Predict(t);
			shotT = (predicted-tankP).magnitude()/shotV;
			if(fabs(shotT-t) < .1 || 
				fabs(predicted.x) * 2.0f > World::instance()->getSize() ||
				fabs(predicted.y) * 2.0f > World::instance()->getSize())
				break;
		}

		//float x = tank->pos[0];
		//float y = tank->pos[1];
		float x = predicted.x;
		float y = predicted.y;

		float r, g, b;
		setColor(tank->color, &r, &g, &b);

		glColor4f(r, g, b, 1.0);
		glBegin(GL_QUADS);
			glVertex3f(x - 1, y - 1, z_index);
			glVertex3f(x + 2, y - 1, z_index);
			glVertex3f(x + 2, y + 2, z_index);
			glVertex3f(x - 1, y + 2, z_index);
		glEnd();

		int step = 1;

		float sigmaX = ktank->Sigma_X();
		float sigmaY = ktank->Sigma_Y();
		float rho = ktank->Rho();

		for (double x = pos.x - box_size / 2; x < pos.x + box_size / 2; x += step) {
			for (double y = pos.y - box_size / 2; y < pos.y + box_size / 2; y += step) {

				float scale = getValAt(pos, x, y, sigmaX, sigmaY, rho) / getValAt(pos, pos.x, pos.y, sigmaX, sigmaY, rho);

				//float r, g, b, a;
				float a = 1;
				setColor(tank->color, &r, &g, &b);
				
				glColor4f(scale * r, scale * g, scale * b, a * scale);
				glBegin(GL_QUADS);
					glVertex3f(x, y, z_index);
					glVertex3f(x + step, y, z_index);
					glVertex3f(x + step, y + step, z_index);
					glVertex3f(x, y + step, z_index);
				glEnd();
			}
		}
	}
}

void draw_world_outline(int z_index) {
	World* world = World::instance();
	int mid = world->getSize() / 2;
	glColor4f(.2, .5, .2, 1.0);
	glBegin(GL_QUADS);
		glVertex3f(-mid, -mid, z_index);
		glVertex3f(mid, -mid, z_index);
		glVertex3f(mid, mid, z_index);
		glVertex3f(-mid, mid, z_index);
	glEnd();
}

void draw_bases(int z_index) {
	World* world = World::instance();

	for (unsigned int i = 0; i < world->teams().size(); i++) {
		team_t* team = &world->teams().at(i);

		float r, g, b;
		setColor(team->color, &r, &g, &b);
		r -= 0.4;
		g -= 0.4;
		b -= 0.4;
		glColor4f(r, g, b, 1.0);
		glBegin(GL_QUADS);
			glVertex3f(team->base_corner[0][0], team->base_corner[0][1], z_index);
			glVertex3f(team->base_corner[1][0], team->base_corner[1][1], z_index);
			glVertex3f(team->base_corner[2][0], team->base_corner[2][1], z_index);
			glVertex3f(team->base_corner[3][0], team->base_corner[3][1], z_index);
		glEnd();
	}
}

void draw_obstacles(int z_index) {
	World* world = World::instance();

	for (unsigned int i = 0; i < world->obstacles().size(); i++) {
		obstacle_t* ob = &world->obstacles().at(i);

		float r, g, b;
		r = g = b = .5;

		glColor4f(r, g, b, 1);
		glBegin(GL_POLYGON);
		for (unsigned int j = 0; j < ob->size; j++) {
			glVertex3f(ob->o_corner[j][0], ob->o_corner[j][1], z_index);
		}
		glEnd();
	}
}

void reshape ( int w, int h )   // Create The Reshape Function (the viewport)
{
  glViewport     ( 0, 0, w, h );
  glMatrixMode   ( GL_PROJECTION );  // Select The Projection Matrix
  glLoadIdentity ( );                // Reset The Projection Matrix
  if ( h==0 ) {  // Calculate The Aspect Ratio Of The Window
     gluPerspective ( 80, ( float ) w, 1.0, 5000.0 );
  }
  else {
     gluPerspective ( 80, ( float ) w / ( float ) h, 1.0, 5000.0 );
  }
  glMatrixMode   ( GL_MODELVIEW );  // Select The Model View Matrix
  glLoadIdentity ( );    // Reset The Model View Matrix
  display();
}

void MouseButton(int button, int state, int x, int y)
{
	// Respond to mouse button presses.
	// If button1 pressed, mark this state so we know in motion function.
	if (button == GLUT_RIGHT_BUTTON)
	{
		button_down = (state == GLUT_DOWN);
	}

	clickx = x;
	clicky = y;
}

void MouseMotion(int x, int y)
{
	if (button_down) {
		zoom += (y - clicky);
	}
	else {
		eyex += (clickx - x);
		eyey += (y - clicky);
	}

	clickx = x;
	clicky = y;
}

void keyboard ( unsigned char key, int x, int y )  // Create Keyboard Function
{
  switch ( key ) {
    case 27:        // When Escape Is Pressed...
      exit ( 0 );   // Exit The Program
      break;        // Ready For Next Case
    default:        // Now Wrap It Up
      break;
  }
}

void arrow_keys ( int a_keys, int x, int y )  // Create Special Function (required for arrow keys)
{
  switch ( a_keys ) {
    case GLUT_KEY_UP:     // When Up Arrow Is Pressed...
      glutFullScreen ( ); // Go Into Full Screen Mode
      break;
    case GLUT_KEY_DOWN:               // When Down Arrow Is Pressed...
      glutReshapeWindow ( 720, 720 ); // Go Into A 720 By 720 Window
      break;
	case GLUT_KEY_HOME:
	  resetCamera();
	  break;
    default:
      break;
  }
}